#include "FastSerial.h"

FastSerialPort0(Serial);        // CLI
FastSerialPort1(Serial1);       // GPS port
FastSerialPort3(Serial3);       // Coms port

#include "Aircraft.h"
#include "Coms.h"
#include <SPI.h>
#include "AP_ADC.h"
#include "AP_PeriodicProcess.h"
#include "AP_IMU.h"
#include "AP_DCM.h"
#include "AP_Compass.h"
#include "AP_InertialSensor.h"
#include "myGps.h"
#include "DataFlash.h"
#include "Ap.h"
#include "PID.h"

Arduino_Mega_ISR_Registry reg;
RC1 rc;
AP_TimerProcess timer;

static GPS *g_gps;

//SENSORS
static AP_ADC_ADS7844 adc;
static AP_Compass_HMC5843 compass;
MyGPS g_gps_driver(&Serial1);
AP_InertialSensor_Oilpan ins(&adc);
AP_IMU_INS imu(&ins);
AP_DCM dcm(&imu, g_gps);

DataFlash_APM1 flash;

Aircraft aircraft;
Coms coms;

void setup()
{
  Serial.begin(115200, 128, 128);
  Serial1.begin(57600, 128, 16);
  Serial.println("START");
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
  reg.init();
  timer.init(&reg);

  adc.Init(&timer);      // APM ADC library initialization
  Serial.println("Initialized ADC");

  pinMode(35, OUTPUT);			// GPS status LED
  pinMode(36, OUTPUT);			// GPS status LED
  pinMode(37, OUTPUT);			// GPS status LED

  //CONFIG RELAY
  DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
  
  //RC STUFF
  rc.init(&reg);
  rc.enable_out(CH_1);
  rc.enable_out(CH_2);
  rc.enable_out(CH_3);
  rc.enable_out(CH_4);
  
  flash.Init();
  
  aircraft.init(&rc, &timer, imu, dcm, g_gps_driver);
  coms.init(&aircraft);
}

void loop()
{
  aircraft.update();
  coms.update();
  if (Serial.available())
  {
	gotoCL();
  }
}
